#ifndef POSE_CONTROLLER_HPP
#define POSE_CONTROLLER_HPP

#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <map>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/float64.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/pose2_d.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include "pid.hpp"

#define PI acos(-1)

struct EulerAngles {
    double roll, pitch, yaw;
};

enum stateControl {
    Rotate,
    MoveForward,
    Default
};

class PoseController : public rclcpp::Node {
private:
    float control_hz;
    float currentyaw;
    float dis_threshold;
    float dis_angle;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twistPublisher_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr feedbackPublisher_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr act_angleSubscriber_;
    rclcpp::Subscription<geometry_msgs::msg::Pose2D>::SharedPtr intSubscriber_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr stopSubscriber_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSubscriber_;
    geometry_msgs::msg::Pose2D Targetpose, Currentpose;
    std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
    std::shared_ptr<tf2_ros::TransformListener> tfListener_;
    PID linear_x;
    PID angular;
    float Lmax, Vmax, kp, ki, kd;
    bool startFlag;
    stateControl stateCtr;
    std::map<std::string, float> params;

    bool poseFlag,quatFlag;

    double target_angle_;
    bool reached_target_;
    double integral_;  
    double previous_error_; 

    rclcpp::TimerBase::SharedPtr watchdog_timer_;
    rclcpp::Time last_odom_time_;  // 记录上次收到 odom 数据的时间

    void readcsv();
    void rosparamsread();
    EulerAngles ToEulerAngles(geometry_msgs::msg::Quaternion orientation);
    void intHandler(const geometry_msgs::msg::Pose2D::SharedPtr msg);
    void stopHandler(const std_msgs::msg::Bool::SharedPtr msg);
    void odomHandler(const nav_msgs::msg::Odometry::SharedPtr msg);
    void actAngleHandler(const std_msgs::msg::Float64::SharedPtr msg);

    void watchdogCallback();

public:
    PoseController();
    void stop();
    void run();
};

#endif // POSE_CONTROLLER_HPP
